/*
 * Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
 *
 * This software is provided 'as-is', without any express or implied
 * warranty.  In no event will the authors be held liable for any damages
 * arising from the use of this software.
 * Permission is granted to anyone to use this software for any purpose,
 * including commercial applications, and to alter it and redistribute it
 * freely, subject to the following restrictions:
 * 1. The origin of this software must not be misrepresented; you must not
 * claim that you wrote the original software. If you use this software
 * in a product, an acknowledgment in the product documentation would be
 * appreciated but is not required.
 * 2. Altered source versions must be plainly marked as such, and must not be
 * misrepresented as being the original software.
 * 3. This notice may not be removed or altered from any source distribution.
 */

package org.box2dflash.common.math {
	import org.box2dflash.common.math.Box2dMath;
	import org.box2dflash.common.math.Mat22;

	public class Box2dMath {
		/// This function is used to ensure that a floating point number is
		/// not a NaN or infinity.
		static public function isValid(x:Number):Boolean {
			return isFinite(x);
		}

		/*static public function InvSqrt(x:Number):Number{
		union
		{
		float32 x;
		int32 i;
		} convert;
		
		convert.x = x;
		float32 xhalf = 0.5f * x;
		convert.i = 0x5f3759df - (convert.i >> 1);
		x = convert.x;
		x = x * (1.5f - xhalf * x * x);
		return x;
		}*/
		static public function dot(a:Vec2, b:Vec2):Number {
			return a.x * b.x + a.y * b.y;
		}

		static public function crossVV(a:Vec2, b:Vec2):Number {
			return a.x * b.y - a.y * b.x;
		}

		static public function crossVF(a:Vec2, s:Number):Vec2 {
			var v:Vec2 = new Vec2(s * a.y, -s * a.x);
			return v;
		}

		static public function crossFV(s:Number, a:Vec2):Vec2 {
			var v:Vec2 = new Vec2(-s * a.y, s * a.x);
			return v;
		}

		static public function mulMV(A:Mat22, v:Vec2):Vec2 {
			// (tMat.col1.x * tVec.x + tMat.col2.x * tVec.y)
			// (tMat.col1.y * tVec.x + tMat.col2.y * tVec.y)
			var u:Vec2 = new Vec2(A.col1.x * v.x + A.col2.x * v.y, A.col1.y * v.x + A.col2.y * v.y);
			return u;
		}

		static public function mulTMV(A:Mat22, v:Vec2):Vec2 {
			// (tVec.x * tMat.col1.x + tVec.y * tMat.col1.y)
			// (tVec.x * tMat.col2.x + tVec.y * tMat.col2.y)
			var u:Vec2 = new Vec2(dot(v, A.col1), dot(v, A.col2));
			return u;
		}

		static public function mulX(T:XForm, v:Vec2):Vec2 {
			var a:Vec2 = mulMV(T.R, v);
			a.x += T.position.x;
			a.y += T.position.y;
			//return T.position + Mul(T.R, v);
			return a;
		}

		static public function mulXT(T:XForm, v:Vec2):Vec2 {
			var a:Vec2 = subtractVV(v, T.position);
			//return MulT(T.R, v - T.position);
			var tX:Number = (a.x * T.R.col1.x + a.y * T.R.col1.y );
			a.y = (a.x * T.R.col2.x + a.y * T.R.col2.y );
			a.x = tX;
			return a;
		}

		static public function addVV(a:Vec2, b:Vec2):Vec2 {
			var v:Vec2 = new Vec2(a.x + b.x, a.y + b.y);
			return v;
		}

		static public function subtractVV(a:Vec2, b:Vec2):Vec2 {
			var v:Vec2 = new Vec2(a.x - b.x, a.y - b.y);
			return v;
		}

		static public function distance(a:Vec2, b:Vec2):Number {
			var cX:Number = a.x - b.x;
			var cY:Number = a.y - b.y;
			return Math.sqrt(cX * cX + cY * cY);
		}

		static public function distanceSquared(a:Vec2, b:Vec2):Number {
			var cX:Number = a.x - b.x;
			var cY:Number = a.y - b.y;
			return (cX * cX + cY * cY);
		}

		static public function mulFV(s:Number, a:Vec2):Vec2 {
			var v:Vec2 = new Vec2(s * a.x, s * a.y);
			return v;
		}

		static public function addMM(A:Mat22, B:Mat22):Mat22 {
			var C:Mat22 = new Mat22(0, addVV(A.col1, B.col1), addVV(A.col2, B.col2));
			return C;
		}

		// A * B
		static public function mulMM(A:Mat22, B:Mat22):Mat22 {
			var C:Mat22 = new Mat22(0, mulMV(A, B.col1), mulMV(A, B.col2));
			return C;
		}

		// A^T * B
		static public function mulTMM(A:Mat22, B:Mat22):Mat22 {
			var c1:Vec2 = new Vec2(dot(A.col1, B.col1), dot(A.col2, B.col1));
			var c2:Vec2 = new Vec2(dot(A.col1, B.col2), dot(A.col2, B.col2));
			var C:Mat22 = new Mat22(0, c1, c2);
			return C;
		}

		static public function abs(a:Number):Number {
			return a > 0.0 ? a : -a;
		}

		static public function absV(a:Vec2):Vec2 {
			var b:Vec2 = new Vec2(abs(a.x), abs(a.y));
			return b;
		}

		static public function absM(A:Mat22):Mat22 {
			var B:Mat22 = new Mat22(0, absV(A.col1), absV(A.col2));
			return B;
		}

		static public function min(a:Number, b:Number):Number {
			return a < b ? a : b;
		}

		static public function minV(a:Vec2, b:Vec2):Vec2 {
			var c:Vec2 = new Vec2(min(a.x, b.x), min(a.y, b.y));
			return c;
		}

		static public function max(a:Number, b:Number):Number {
			return a > b ? a : b;
		}

		static public function maxV(a:Vec2, b:Vec2):Vec2 {
			var c:Vec2 = new Vec2(max(a.x, b.x), max(a.y, b.y));
			return c;
		}

		static public function clamp(a:Number, low:Number, high:Number):Number {
			return max(low, min(a, high));
		}

		static public function clampV(a:Vec2, low:Vec2, high:Vec2):Vec2 {
			return maxV(low, minV(a, high));
		}

		static public function swap(a:Array, b:Array):void {
			var tmp:* = a[0];
			a[0] = b[0];
			b[0] = tmp;
		}

		// Random number in range [-1,1]
		static public function random():Number {
			return Math.random() * 2 - 1;
		}

		static public function randomRange(lo:Number, hi:Number):Number {
			var r:Number = Math.random();
			r = (hi - lo) * r + lo;
			return r;
		}

		// "Next Largest Power of 2
		// Given a binary integer value x, the next largest power of 2 can be computed by a SWAR algorithm
		// that recursively "folds" the upper bits into the lower bits. This process yields a bit vector with
		// the same most significant 1 as x, but all 1's below it. Adding 1 to that value yields the next
		// largest power of 2. For a 32-bit value:"
		static public function nextPowerOfTwo(x:uint):uint {
			x |= (x >> 1) & 0x7FFFFFFF;
			x |= (x >> 2) & 0x3FFFFFFF;
			x |= (x >> 4) & 0x0FFFFFFF;
			x |= (x >> 8) & 0x00FFFFFF;
			x |= (x >> 16) & 0x0000FFFF;
			return x + 1;
		}

		static public function isPowerOfTwo(x:uint):Boolean {
			var result:Boolean = x > 0 && (x & (x - 1)) == 0;
			return result;
		}

		// Temp vector functions to reduce calls to 'new'
		/*static public var tempVec:Vec2 = new Vec2();
		static public var tempVec2:Vec2 = new Vec2();
		static public var tempVec3:Vec2 = new Vec2();
		static public var tempVec4:Vec2 = new Vec2();
		static public var tempVec5:Vec2 = new Vec2();
	
		static public var tempMat:Mat22 = new Mat22();	
	
		static public var tempAABB:AABB = new AABB();	*/
		static public const Vec2_zero:Vec2 = new Vec2(0.0, 0.0);
		static public const Mat22_identity:Mat22 = new Mat22(0, new Vec2(1.0, 0.0), new Vec2(0.0, 1.0));
		static public const XForm_identity:XForm = new XForm(Vec2_zero, Mat22_identity);
	}
}
